{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "EgiF12Hf1Dhs"
   },
   "source": [
    "This notebook provides examples to go along with the [textbook](http://manipulation.csail.mit.edu/robot.html).  I recommend having both windows open, side-by-side!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "eeMrMI0-1Dhu"
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from pydrake.all import (\n",
    "    AddMultibodyPlantSceneGraph,\n",
    "    DiagramBuilder,\n",
    "    InverseDynamicsController,\n",
    "    LeafSystem,\n",
    "    MeshcatVisualizer,\n",
    "    MultibodyPlant,\n",
    "    Parser,\n",
    "    Simulator,\n",
    "    StartMeshcat,\n",
    "    StateInterpolatorWithDiscreteDerivative,\n",
    ")\n",
    "from manipulation.station import MakeHardwareStation, load_scenario\n",
    "from manipulation.utils import RenderDiagram"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start the visualizer.\n",
    "meshcat = StartMeshcat()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "wJWL-ldv5REK"
   },
   "source": [
    "# Controlling the iiwa with an allegro hand\n",
    "\n",
    "The `MakeHardwareStation` code sets everything up assuming that you have an iiwa with a Schunk WSG gripper. What if you want to use the allegro hand instead? Then it probably makes sense implement the basic components of `MakeHardwareStation` yourself.\n",
    "\n",
    "The simplest approach, which will be suitable for simulation, will be to use one `InverseDynamicsController` that treats the iiwa + Allegro as a single robot to control. If you want to run on the actual iiwa hardware, then we can do better (create two controllers, one for the iiwa which assumes an equivalent mass for the hand in a nominal fixed position + another controller for the hand), but that is not necessary to start."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "9GSF52A8ExQa"
   },
   "outputs": [],
   "source": [
    "scenario_data = \"\"\"\n",
    "directives:\n",
    "- add_model:\n",
    "    name: iiwa\n",
    "    file: package://drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf\n",
    "    default_joint_positions:\n",
    "        iiwa_joint_1: [0]\n",
    "        iiwa_joint_2: [0.1]\n",
    "        iiwa_joint_3: [0]\n",
    "        iiwa_joint_4: [-1.2]\n",
    "        iiwa_joint_5: [0]\n",
    "        iiwa_joint_6: [ 1.6]\n",
    "        iiwa_joint_7: [0]\n",
    "- add_weld:\n",
    "    parent: world\n",
    "    child: iiwa::iiwa_link_0\n",
    "- add_model:\n",
    "    name: allegro\n",
    "    file: package://drake/manipulation/models/allegro_hand_description/sdf/allegro_hand_description_right.sdf\n",
    "- add_weld:\n",
    "    parent: iiwa::iiwa_link_7\n",
    "    child: allegro::hand_root\n",
    "    X_PC:\n",
    "        translation: [0, 0, 0.05]\n",
    "        rotation: !Rpy { deg: [0, 0, 0]}\n",
    "# And now the environment:\n",
    "- add_model:\n",
    "    name: foam_brick\n",
    "    file: package://drake/examples/manipulation_station/models/061_foam_brick.sdf\n",
    "    default_free_body_pose:\n",
    "        base_link:\n",
    "            translation: [0.6, 0, 0]\n",
    "- add_model:\n",
    "    name: robot_table\n",
    "    file: package://drake/examples/kuka_iiwa_arm/models/table/extra_heavy_duty_table_surface_only_collision.sdf\n",
    "- add_weld:\n",
    "    parent: world\n",
    "    child: robot_table::link\n",
    "    X_PC:\n",
    "        translation: [0, 0, -0.7645]\n",
    "- add_model:\n",
    "    name: work_table\n",
    "    file: package://drake/examples/kuka_iiwa_arm/models/table/extra_heavy_duty_table_surface_only_collision.sdf\n",
    "- add_weld:\n",
    "    parent: world\n",
    "    child: work_table::link\n",
    "    X_PC:\n",
    "        translation: [0.75, 0, -0.7645]\n",
    "model_drivers:\n",
    "    iiwa+allegro: !InverseDynamicsDriver {}\n",
    "\"\"\"\n",
    "\n",
    "scenario = load_scenario(data=scenario_data)\n",
    "station = MakeHardwareStation(scenario, meshcat)\n",
    "\n",
    "RenderDiagram(station, max_depth=1)\n",
    "\n",
    "simulator = Simulator(station)\n",
    "context = simulator.get_mutable_context()\n",
    "\n",
    "x0 = station.GetOutputPort(\"iiwa+allegro.state_estimated\").Eval(context)\n",
    "station.GetInputPort(\"iiwa+allegro.desired_state\").FixValue(context, x0)\n",
    "\n",
    "# Confirm that simulation works:\n",
    "simulator.AdvanceTo(0.1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "colab": {
   "collapsed_sections": [],
   "name": "Robotic Manipulation - Let's get you a robot.ipynb",
   "provenance": [],
   "toc_visible": true
  },
  "kernelspec": {
   "display_name": "Python 3.10.8 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.6"
  },
  "vscode": {
   "interpreter": {
    "hash": "b0fa6594d8f4cbf19f97940f81e996739fb7646882a419484c72d19e05852a7e"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
